package testApplets;

import grid.Grid3DDepth;
import grid.Grid3DFloat;
import grid.Grid3DTraffic;
import grid.Grid3DView;
import hdgeom.primitives.Rect;
import hdgeom.procedures.ConvexHull;
import hdgeom.procedures.POrientedBoundingBox;

import java.util.ArrayList;
import java.util.Collections;
import java.util.Vector;

import analyses.SunLightCalculator;
import blobDetector.Blob;
import blobDetector.BlobDetector;
import SimpleOpenNI.SimpleOpenNI;
import peasy.PeasyCam;
import processing.core.PApplet;
import processing.core.PGraphics;
import processing.core.PImage;
import processing.core.PVector;
import simpleGeom.HasPos2D;
import simpleGeom.Vec2D;

public class TestSimpleOpenNIKinect extends PApplet {
	PeasyCam cam;
	SimpleOpenNI context;
	BlobDetector blobDetect;
	float zoomF = 0.3f;
	float rotX = radians(180); // by default rotate the hole scene 180deg around
								// the x-axis,
								// the data from openni comes upside down
	float rotY = radians(0);
	float[] brightnessArray = new float[640 * 480];
	PImage img;
	PImage analyseImage;
	PGraphics canvas;
	float rx1 = 210;
	float ry1 = 208;
	Rect sourceRect = new Rect(rx1, ry1, rx1 + 212, ry1 + 144);
	boolean doCam=true;
	float maxArea=30000;
	float minArea;
	ArrayList<POrientedBoundingBox> boxes = new ArrayList<POrientedBoundingBox>();
	ArrayList<Float> heights=new ArrayList<Float>();
	float sc;
	float tx;
	float ty;
	float minH;
	float maxH;
	public void setup() {
		if (doCam){
			size(1280, 800);
			startKinect();
		}
		else{
			size(640, 400);
		}
		stroke(255, 255, 255);
		smooth();
		blobDetect = new BlobDetector(640, 480);
		//img = this.loadImage("/Users/dillenburger_b/Desktop/test2.jpg");
		canvas = this.createGraphics((int) (sourceRect.getDimX() * 2f),
				(int) (sourceRect.getDimY() * 2));
	}

	public void sunShadow() {
		canvas.beginDraw();
		canvas.background(255);
		SunLightCalculator sun=new SunLightCalculator();
		canvas.stroke(0,0,255);
		sc = canvas.width * 1f / sourceRect.getDimX();
		tx = -sourceRect.x1 * sc;
		ty = sourceRect.y2 * sc;
		canvas.translate(tx, ty);
		canvas.scale(sc,-sc);
		canvas.noStroke();
		canvas.fill(255,0,0);
		for (int h = 1; h < 2; h++) {
			double[] res=sun.getAzimutElevation(2000, 3, 1, h+9, 0, 0);
			double[]vec=SunLightCalculator.getShadowVector(res);
			vec[0]*=5;
			vec[1]*=5;
			for (int i = 0; i < boxes.size(); i++) {
				POrientedBoundingBox box = boxes.get(i);
				box.display2D(canvas);
				canvas.beginShape(QUAD_STRIP);
				for (int j=0;j<box.getNumVertices()+1;j++){
					PVector v1=box.getVertex(j%box.getNumVertices());
					canvas.vertex(v1.x,v1.y);
					canvas.vertex(v1.x+(float)vec[0],v1.y+(float)vec[1]);
				}
				canvas.endShape();
				canvas.pushMatrix();
				canvas.translate((float)vec[0],(float) vec[1]);
				box.display2D(canvas);
				canvas.popMatrix();
			}
		}
		canvas.endDraw();
	}
	public void traffic(){
		canvas.beginDraw();
		canvas.background(255);
		sc = canvas.width * 1f / sourceRect.getDimX();
		tx = -sourceRect.x1 * sc;
		ty = sourceRect.y2 * sc;
		canvas.pushMatrix();
		canvas.translate(tx, ty);
		canvas.scale(sc,-sc);
		canvas.fill(220);
		for (int i = 0; i < boxes.size(); i++) {
			POrientedBoundingBox box = boxes.get(i);
			box.display2D(canvas);
		}
		canvas.endDraw();
		println("here");
		Grid3DFloat gridDistance=new Grid3DFloat(canvas.width,canvas.height,1);
		for (int x=0;x<canvas.width;x++){
			for (int y=0;y<canvas.height;y++){
				float b=brightness(canvas.get(x, y));
				if (b<230)gridDistance.set(x, y,0,0);
				else gridDistance.set(x, y,0,10000);
			}
		}
		Grid3DDepth.calculateDepth(gridDistance);
		for (int i=0;i<gridDistance.values.length;i++){
			if (gridDistance.values[i]>5)gridDistance.values[i]=1;
			else gridDistance.values[i]=10;
		}
		canvas.beginDraw();
		Grid3DTraffic grid=new Grid3DTraffic(canvas.width,canvas.height,1);
		grid.setValueToAll(10000);
		for (int i = 0; i < 1; i++) {
			POrientedBoundingBox box = boxes.get(i);
			box.display2D(canvas);
			PVector center=box.getCenter();
			canvas.rect(center.x,center.y,5,5);
			int x=(int)((center.x*sc+tx));
			int y=(int)((center.y*-sc+ty));
			if (x>=0&&x<grid.nX&&y>=0&&y<grid.nY){
				grid.set(x, y,0,0);
			}
		}
		grid.distance2D(gridDistance.values);
		canvas.colorMode(RGB,255);
		canvas.popMatrix();
		canvas.noStroke();
		for (int i = 0; i < 200; i++) {
			POrientedBoundingBox box = boxes.get((int)random(boxes.size()));
			PVector center=box.getCenter();
			int x=(int)((center.x*sc+tx));
			int y=(int)((center.y*-sc+ty));
			int nb = grid.getIndex(x, y, 0);
			int nSteps=0;
			while (nb > 0 && grid.get(nb) > 0&&nSteps<1000) {
				canvas.fill(255,0,0,5);
				canvas.rect(grid.getX(nb), grid.getY(nb),2,2);
				//canvas.set(grid.getX(nb), grid.getY(nb), color(255, 0, 0));
				nb = grid.getCloserNb2D(nb,0.73f);
				nSteps++;
			}
		}
		canvas.endDraw();
	}
	public void detect(){
		
	}
	public void view() {
		println("view");
		canvas.beginDraw();
		canvas.background(255);
		sc = canvas.width * 1f / sourceRect.getDimX();
		tx = -sourceRect.x1 * sc;
		ty = sourceRect.y2 * sc;
		canvas.translate(tx, ty);
		canvas.scale(sc,-sc);
		canvas.noStroke();
		canvas.fill(0);
		for (int i = 0; i < boxes.size(); i++) {
			POrientedBoundingBox box = boxes.get(i);
			float h=heights.get(i);
			canvas.fill(map(h,minH,maxH,0,200));
			box.display2D(canvas);
		}
		Grid3DFloat obstacles=new Grid3DFloat(canvas.width,canvas.height,1);
		for (int x=0;x<canvas.width;x++){
			for (int y=0;y<canvas.height;y++){
				float b=brightness(canvas.get(x, y));
				if (b<253)obstacles.set(x, y,0,1);
				else obstacles.set(x, y,0,0);	
			}
		}
		Grid3DView view=new Grid3DView(obstacles.nX,obstacles.nY,obstacles.nZ);
		view.viewAnalyse(obstacles, 170, 90);
		
		for (int x = 0; x < canvas.width; x++) {
			for (int y = 0; y < canvas.height; y++) {
				float d = view.get(x, y, 0);
				if (d > 0) {
					canvas.set(x, y, canvas.color(0, 255, 0));
				}
			}
		}
		canvas.endDraw();
	}
	public void distance() {
		canvas.beginDraw();
		canvas.background(255);
		sc = canvas.width * 1f / sourceRect.getDimX();
		tx = -sourceRect.x1 * sc;
		ty = sourceRect.y2 * sc;
		canvas.translate(tx, ty);
		canvas.scale(sc,-sc);
		canvas.noStroke();
		canvas.fill(0);
		for (int i = 0; i < boxes.size(); i++) {
			POrientedBoundingBox box = boxes.get(i);
			box.display2D(canvas);
		}
		canvas.endDraw();
		
		Grid3DFloat grid=new Grid3DFloat(canvas.width,canvas.height,1);
		for (int x=0;x<canvas.width;x++){
			for (int y=0;y<canvas.height;y++){
				float b=brightness(canvas.get(x, y));
				if (b<10){
					grid.set(x, y,0,0);
					
				}
				else{
					grid.set(x, y,0,10000);
					
				}
			}
		}
		canvas.beginDraw();
		Grid3DDepth.calculateDepth(grid);
		canvas.colorMode(HSB,255);
		for (int x=0;x<canvas.width;x++){
			for (int y=0;y<canvas.height;y++){
				float d=grid.get(x, y, 0);
				//println(d);
				canvas.set(x,y,canvas.color((d*20)%255,255,255));
			}
		}
		canvas.endDraw();
	}
	
	public void voronoi() {
		canvas.beginDraw();
		canvas.background(0);
		sc = canvas.width * 1f / sourceRect.getDimX();
		tx = -sourceRect.x1 * sc;
		ty = sourceRect.y2 * sc;
		canvas.translate(tx, ty);
		canvas.scale(sc,-sc);
		canvas.noStroke();
		canvas.colorMode(RGB,255);
		println("nObjects: "+boxes.size());
		for (int i = 0; i < boxes.size(); i++) {
			canvas.fill(i*10,0,0);
			POrientedBoundingBox box = boxes.get(i);
			box.display2D(canvas);
		}
		
		canvas.background(0);
		Grid3DFloat grid=new Grid3DFloat(canvas.width,canvas.height,1);
		Grid3DFloat gridObjects=new Grid3DFloat(canvas.width,canvas.height,1);
		for (int x=0;x<canvas.width;x+=2){
			for (int y=0;y<canvas.height;y+=2){
				float cX=x*sc+tx;
				float cY=y*-sc+ty;
				for (int i = 0; i < boxes.size(); i++) {
					POrientedBoundingBox box = boxes.get(i);
					if (box.contains(cX, cY)){
						//println("inside");
						canvas.fill(i*10,0,255);
						canvas.rect(cX,cY,4,4);
						//canvas.set(x, y, color(i*10,0,0));
					}
				}
			}
		}
		canvas.endDraw();
		//println("here");
		for (int x=0;x<canvas.width;x++){
			for (int y=0;y<canvas.height;y++){
				float b=red(canvas.get(x, y));
				//println(b);
				int bV=(int)(b/5);
				
				//println(bV);
				if (b>5&&b%10==0){
					//println("found");
					grid.set(x, y,0,0);
					gridObjects.set(x, y,0,bV);
				}
				else{
					grid.set(x, y,0,10000);
					gridObjects.set(x, y,0,-1);
					
				}
			}
		}
	
		Grid3DDepth.calculateDepth(gridObjects.values, grid.values, grid.nX, grid.nY, 1, grid.d1, grid.d2, grid.d3);
		//Grid3DDepth.calculateDepth(grid);
		canvas.beginDraw();
		canvas.colorMode(HSB,255);
		for (int x=0;x<canvas.width;x++){
			for (int y=0;y<canvas.height;y++){
				float d=gridObjects.get(x, y, 0);
				if (gridObjects.hasDifferentNb2D(x, y, 0)){
					canvas.set(x,y,canvas.color(255,0,255));
				}
				else{
				canvas.set(x,y,canvas.color((d*2),255,255));
				}
			}
		}
		canvas.endDraw();
	}

	public void startKinect() {
		context = new SimpleOpenNI(this);
		if (context.isInit() == false) {
			println("Can't init SimpleOpenNI, maybe the camera is not connected!");
			exit();
			return;
		}
		// disable mirror
		context.setMirror(true);
		// enable depthMap generation
		context.enableDepth();
		context.enableRGB();
	}

	public void draw() {
		minH=255;
		maxH=0;
		background(0, 0, 0);
		sc = width * 1f / sourceRect.getDimX();
		tx = -sourceRect.x1 * sc;
		ty = sourceRect.y2 * sc;
		pushMatrix();
		translate(tx, ty);
		scale(sc, -sc);
		// update the cam
		if (doCam) {
			context.update();
			int[] depthMap = context.depthMap();
			PImage depth = context.depthImage();
			 img = context.rgbImage();
			img = depth;
			image(depth, 0, 0);
		} else {
			image(img, 0, 0);
		}
		noFill();

		for (int i = 0; i < img.pixels.length; i++) {
			brightnessArray[i] = brightness(img.pixels[i]);
		}
		blobDetect.setValues(brightnessArray);
		blobDetect.calculateBlobs(200);
		Vector<Blob> blobs = blobDetect.getBlobs();
		boxes = new ArrayList<POrientedBoundingBox>();
		heights=new ArrayList<Float>();
		strokeWeight(0.2f);
		for (int i = 0; i < blobs.size(); i++) {
			Blob bl = blobs.get(i);
			stroke(255, 0, 0);
			//bl.display2D(g);

			Vector<HasPos2D> points = new Vector<HasPos2D>();
			for (int ii = 0; ii < bl.getNumVertices(); ii++) {
				PVector v = bl.getVertex(ii);
				points.add(new Vec2D(v.x, v.y));
			}
			if (bl.getArea() > 0) {
				Collections.reverse(points);
				ConvexHull cHull = new ConvexHull(points);
				stroke(255,0,0);
				POrientedBoundingBox pBox = new POrientedBoundingBox(cHull.hull);
				float area = pBox.getArea();
				PVector c = pBox.getCenter();
				if (sourceRect.contains(c.x, c.y)) {
					if (area < maxArea) {
						PVector center=pBox.getCenter();
						int x1=(int)center.x-5;
						int y1=(int)center.y-5;
						int x2=(int)center.x+5;
						int y2=(int)center.y+5;
						int nC=0;
						int avHeight=0;
						for (int x=x1;x<x2;x++){
							for (int y=y1;y<y2;y++){
								float d=brightness(img.get(x, y));
								nC++;
								avHeight+=d;
							}
						}
						avHeight=avHeight/nC;
						if (avHeight<minH)minH=avHeight;
						if (avHeight>maxH)maxH=avHeight;
						this.heights.add(new Float(avHeight));
						println("height: "+avHeight);
						boxes.add(pBox);
					}
				}
				stroke(255, 0, 0);
				/*beginShape();
				for (int ii = 0; ii < pBox.convexHull.size(); ii++) {
					HasPos2D p = pBox.convexHull.get(ii);
					vertex(p.getX(), p.getY());
				}
				endShape(CLOSE);*/
				stroke(255, 0, 0);
				pBox.display2D(g);
			}
		}

		stroke(255, 255, 0);
		
		sourceRect.display(g);
		popMatrix();
		//sunShadow();
		//voronoi();
		//distance();
		//view();
		//traffic();
		pushMatrix();
		float cScale = width * 1f / canvas.width;
		scale(cScale);
		//image(canvas, 0, 0);
		popMatrix();

		/*
		 * int steps = 2; // to speed up the drawing, draw every third point int
		 * index; PVector realWorldPoint; PVector[] realWorldMap =
		 * context.depthMapRealWorld(); for (int y = 0; y <
		 * context.depthHeight(); y += steps) { for (int x = 0; x <
		 * context.depthWidth(); x += steps) { index = x + y *
		 * context.depthWidth(); if (depthMap[index] > 0) { realWorldPoint =
		 * realWorldMap[index]; stroke(img.get(x, y)); point(realWorldPoint.x,
		 * realWorldPoint.y, realWorldPoint.z); // eye } } }
		 * context.drawCamFrustum();
		 */
	}

}
